﻿/*
 * This file is part of CommunitySettlers.
 *
 * CommunitySettlers is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * CommunitySettlers is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with CommunitySettlers.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * Author: Christoph Husse
 * 
 * Also checkout our homepage: http://communitysettlers.codeplex.com/
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace SettlersEngine
{
    internal interface IPathReservation : IDisposable {
        long ReplanTime { get; set; }
        int MovableType { get; }
        int CycleSpeed { get; }
        void Invalidate();
    }

    internal delegate void DOnPathNotification(Object inCallback);

    /// <summary>
    /// Implementation of paper http://webdocs.cs.ualberta.ca/~silver/David_Silver/Publications_files/coop-path-AIWisdom.pdf.
    /// </summary>
    /// <typeparam name="TPathNode"></typeparam>
    internal class CooperativeAStar<TPathNode>
        where TPathNode : NativeEngine.IAStarNode
    {
        private static readonly Double SQRT_2 = Math.Sqrt(2);
        private SortedDictionary<PathNodeKey, Object> m_ClosedSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
        private SortedDictionary<PathNodeKey, Object> m_OpenSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
        private PriorityQueue m_OrderedOpenSet = new PriorityQueue();
        private SortedDictionary<PathNodeKey, PathNodeKey> m_CameFrom = new SortedDictionary<PathNodeKey, PathNodeKey>(PathNodeKey.Comparer);
        private Double m_PauseCosts = 0.1;
        private NativeEngine.AStarSolver<TPathNode> m_BackwardAStar;
        private readonly SpaceTimeMap m_Reservation = new SpaceTimeMap();
        
        public const Double MIN_PAUSE_COSTS = 0.1;

        public Int64 CurrentCycle { get; private set; }

        /// <summary>
        /// Pause costs decide whether the algorithm is likely to not move a unit at all if
        /// there is an obstacle (low pause costs) or find a way round (high pause costs).
        /// Note that a unit is only pausable, if it interacts with reserved cells, meaning
        /// other moving units. It won't pause for a static obstacles.
        /// </summary>
        public Double PauseCosts
        {
            get { return m_PauseCosts; }
            set
            {
                if (value <= MIN_PAUSE_COSTS)
                    throw new ArgumentOutOfRangeException();

                m_PauseCosts = value;
            }
        }
        public Int64 CycleResolution { get; private set; }
        

        public int Width { get { return m_BackwardAStar.Width; } }
        public int Height { get { return m_BackwardAStar.Height; } }
        public TPathNode[,] SearchSpace { get { return m_BackwardAStar.SearchSpace; } }

        internal struct PathNodeKey
        {
            public int X;
            public int Y;
            public long Time;
            public double F;
            public double G;
            public double H;
            public int Index;

            public static KeyComparer Comparer = new KeyComparer();
            public static KeyComparerIgnoreTime ComparerIgnoreTime = new KeyComparerIgnoreTime();

            public class KeyComparer : IComparer<PathNodeKey>
            {
                public int Compare(PathNodeKey a, PathNodeKey b)
                {
                    if (a.X < b.X)
                        return -1;
                    else if (a.X > b.X)
                        return 1;

                    if (a.Y < b.Y)
                        return -1;
                    else if (a.Y > b.Y)
                        return 1;

                    // must be the last comparison
                    if (a.Time < b.Time)
                        return -1;
                    else if (a.Time > b.Time)
                        return 1;

                    return 0;
                }
            }

            public class KeyComparerIgnoreTime : IComparer<PathNodeKey>
            {
                public int Compare(PathNodeKey a, PathNodeKey b)
                {
                    if (a.X < b.X)
                        return -1;
                    else if (a.X > b.X)
                        return 1;

                    if (a.Y < b.Y)
                        return -1;
                    else if (a.Y > b.Y)
                        return 1;

                    return 0;
                }
            }

            public PathNodeKey(int inX, int inY, long inTime)
            {
                F = 0;
                H = 0;
                G = 0;
                X = inX;
                Y = inY;
                Time = inTime;
                Index = -1;
            }
        }

        protected TPathNode this[PathNodeKey key]
        {
            get
            {
                return SearchSpace[key.X, key.Y];
            }
        }

        protected TPathNode this[System.Drawing.Point key]
        {
            get
            {
                return SearchSpace[key.X, key.Y];
            }
        }

        public CooperativeAStar(TPathNode[,] inSearchSpace, Int64 inCurrentCycle, Int64 inCycleResolution)
        {
            CycleResolution = inCycleResolution;
            CurrentCycle = inCurrentCycle;
            m_BackwardAStar = new NativeEngine.AStarSolver<TPathNode>(inSearchSpace);
        }

        public void AddOneCycle()
        {
            CurrentCycle++;
        }

        public void RemoveReservation(IEnumerable<PathNodeKey> inPath)
        {
            foreach (PathNodeKey node in inPath)
            {
                m_Reservation.Remove(node);
            }
        }

        public void AddReservation(IPathReservation inReservation, IEnumerable<PathNodeKey> inPath)
        {
            Int64 cycle = CurrentCycle / CycleResolution;

            foreach (var node in inPath)
            {
                for (int i = 0; i < inReservation.CycleSpeed; i++)
                {
                    PathNodeKey key = new PathNodeKey(node.X, node.Y, cycle * CycleResolution);
                    IPathReservation cell = m_Reservation[key];

                    if (cell != null)
                    {
                        if (cell.MovableType >= inReservation.MovableType)
                            throw new ApplicationException();

                        cell.Invalidate();
                    }

                    m_Reservation[key] = inReservation;
                    inReservation.ReplanTime = key.Time;

                    cycle++;
                }
            }
        }

        protected Double Heuristic(PathNodeKey inStart)
        {
            return m_BackwardAStar.GetGValue(new System.Drawing.Point(inStart.X, inStart.Y));
        }

        protected virtual Double NeighborDistance(PathNodeKey inStart, PathNodeKey inEnd)
        {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY)
            {
                case 1: return 1;
                case 2: return SQRT_2;
                case 0: return PauseCosts;
                default:
                    throw new ApplicationException();
            }
        }

        public IEnumerable<TPathNode> Search(
            Int32 inMovableCycleSpeed,
            System.Drawing.Point inStart,
            System.Drawing.Point inCurrent,
            System.Drawing.Point inEnd,
            int inMovableType)
        {
            return from e in SearchRaw(inMovableCycleSpeed, inStart, inCurrent, inEnd, inMovableType) select this[e];
        }

        private int PATH_PREDICTION = 12;

        public LinkedList<PathNodeKey> SearchRaw(
            Int32 inMovableCycleSpeed,
            System.Drawing.Point inStart,
            System.Drawing.Point inCurrent,
            System.Drawing.Point inEnd,
            int inMovableType)
        {
            if ((inMovableCycleSpeed < 1) || (inMovableCycleSpeed > 1000))
                throw new ArgumentOutOfRangeException();

            // initialize backward search for heuristic
            m_BackwardAStar.BeginInstancing(inEnd, inStart, inMovableType);

            // clear state
            m_ClosedSet.Clear();
            m_OpenSet.Clear();
            m_OrderedOpenSet.Clear();
            m_CameFrom.Clear();

            Debug.Assert((CurrentCycle % CycleResolution) == 0);

            PathNodeKey startNodeKey = new PathNodeKey(inCurrent.X, inCurrent.Y, (CurrentCycle / CycleResolution) * CycleResolution);

            startNodeKey.G = 0;
            startNodeKey.H = Heuristic(startNodeKey);
            startNodeKey.F = startNodeKey.H;

            m_OpenSet.Add(startNodeKey, null);
            m_OrderedOpenSet.Push(startNodeKey);

            // start search
            PathNodeKey[] neighborNodes = new PathNodeKey[9];
            LinkedList<PathNodeKey> result = null;

            while (m_OpenSet.Count > 0)
            {
                PathNodeKey x = m_OrderedOpenSet.Pop();

                if ((x.X == inEnd.X) && (x.Y == inEnd.Y))
                {
                    int count;

                    if((count = CountPathNodes(m_CameFrom, m_CameFrom[x])) >= PATH_PREDICTION)
                    {
                        result = ReconstructPath(m_CameFrom, m_CameFrom[x]);
                        result.AddLast(x);

                        return result;
                    }
                }

                m_OpenSet.Remove(x);
                m_ClosedSet.Add(x, null);

                StoreNeighborNodes(x, neighborNodes, inMovableCycleSpeed * CycleResolution);

                for (int i = 0; i < neighborNodes.Length; i++)
                {
                    PathNodeKey y = neighborNodes[i];
                    Boolean tentative_is_better;

                    if (y.Time == 0)
                        continue;

                    if (!m_BackwardAStar.IsWalkable(new System.Drawing.Point(y.X, y.Y), inMovableType))
                        continue;

                    if (m_ClosedSet.ContainsKey(y))
                        continue;

                    IPathReservation reserved = m_Reservation[y];

                    if (reserved != null)
                        continue;

                    Double tentative_g_score = x.G + NeighborDistance(x, y);
                    Boolean wasAdded = false;

                    if (!m_OpenSet.ContainsKey(y))
                    {
                        m_OpenSet.Add(y, null);
                        tentative_is_better = true;
                        wasAdded = true;
                    }
                    else if (tentative_g_score < y.G)
                    {
                        tentative_is_better = true;
                    }
                    else
                    {
                        tentative_is_better = false;
                    }

                    if (tentative_is_better)
                    {
                        m_CameFrom.Add(y, x);

                        y.G = tentative_g_score;
                        y.H = Heuristic(y);
                        y.F = y.G + y.H;

                        if (wasAdded)
                            m_OrderedOpenSet.Push(y);
                        else
                            m_OrderedOpenSet.Update(y);
                    }
                }
            }

            return result;
        }

        private LinkedList<PathNodeKey> ReconstructPath(SortedDictionary<PathNodeKey, PathNodeKey> came_from, PathNodeKey current_node)
        {
            LinkedList<PathNodeKey> result = new LinkedList<PathNodeKey>();

            ReconstructPathRecursive(came_from, current_node, result);

            return result;
        }

        private void ReconstructPathRecursive(SortedDictionary<PathNodeKey, PathNodeKey> came_from, PathNodeKey current_node, LinkedList<PathNodeKey> result)
        {
            PathNodeKey item;

            if (came_from.TryGetValue(current_node, out item))
            {
                ReconstructPathRecursive(came_from, item, result);

                result.AddLast(current_node);
            }
            else
                result.AddLast(current_node);
        }

        private int CountPathNodes(SortedDictionary<PathNodeKey, PathNodeKey> came_from, PathNodeKey current_node)
        {
            PathNodeKey item;

            if (came_from.TryGetValue(current_node, out item))
            {
                return CountPathNodes(came_from, item) + 1;
            }
            else
                return 1;
        }

        private void StoreNeighborNodes(PathNodeKey inAround, PathNodeKey[] inNeighbors, long inCycleSpeed)
        {
            int x = inAround.X;
            int y = inAround.Y;
            long t = inAround.Time + inCycleSpeed;

            if ((x > 0) && (y > 0))
                inNeighbors[0] = new PathNodeKey(x - 1, y - 1, t);
            else
                inNeighbors[0] = default(PathNodeKey);

            if (y > 0)
                inNeighbors[1] = new PathNodeKey(x, y - 1, t);
            else
                inNeighbors[1] = default(PathNodeKey);

            if ((x < Width - 1) && (y > 0))
                inNeighbors[2] = new PathNodeKey(x + 1, y - 1, t);
            else
                inNeighbors[2] = default(PathNodeKey);

            if (x > 0)
                inNeighbors[3] = new PathNodeKey(x - 1, y, t);
            else
                inNeighbors[3] = default(PathNodeKey);

            if (x < Width - 1)
                inNeighbors[4] = new PathNodeKey(x + 1, y, t);
            else
                inNeighbors[4] = default(PathNodeKey);

            if ((x > 0) && (y < Height - 1))
                inNeighbors[5] = new PathNodeKey(x - 1, y + 1, t);
            else
                inNeighbors[5] = default(PathNodeKey);

            if (y < Height - 1)
                inNeighbors[6] = new PathNodeKey(x, y + 1, t);
            else
                inNeighbors[6] = default(PathNodeKey);

            if ((x < Width - 1) && (y < Height - 1))
                inNeighbors[7] = new PathNodeKey(x + 1, y + 1, t);
            else
                inNeighbors[7] = default(PathNodeKey);

            inNeighbors[8] = new PathNodeKey(x, y, t);
        }

        private class PriorityQueue
        {
            private List<PathNodeKey> InnerList = new List<PathNodeKey>();

            public PriorityQueue()
            {
            }

            protected void SwitchElements(int i, int j)
            {
                PathNodeKey ik = InnerList[i];
                PathNodeKey jk = InnerList[j];

                ik.Index = j;
                jk.Index = i;

                InnerList[i] = jk;
                InnerList[j] = ik;
            }

            protected virtual int OnCompare(int i, int j)
            {
                PathNodeKey ik = InnerList[i];
                PathNodeKey jk = InnerList[j];

                if (ik.F < jk.F)
                    return -1;
                else if (ik.F > jk.F)
                    return 1;
                else
                    return 0;
            }

            /// <summary>
            /// Push an object onto the PQ
            /// </summary>
            /// <param name="O">The new object</param>
            /// <returns>The index in the list where the object is _now_. This will change when objects are taken from or put onto the PQ.</returns>
            public int Push(PathNodeKey item)
            {
                int p = InnerList.Count, p2;
                item.Index = InnerList.Count;
                InnerList.Add(item); // E[p] = O

                do
                {
                    if (p == 0)
                        break;
                    p2 = (p - 1) / 2;
                    if (OnCompare(p, p2) < 0)
                    {
                        SwitchElements(p, p2);
                        p = p2;
                    }
                    else
                        break;
                } while (true);
                return p;
            }

            /// <summary>
            /// Get the smallest object and remove it.
            /// </summary>
            /// <returns>The smallest object</returns>
            public PathNodeKey Pop()
            {
                PathNodeKey result = InnerList[0];
                int p = 0, p1, p2, pn;
                PathNodeKey i0 = InnerList[InnerList.Count - 1];

                i0.Index = 0;
                InnerList[0] = i0;

                InnerList.RemoveAt(InnerList.Count - 1);

                result.Index = -1;

                do
                {
                    pn = p;
                    p1 = 2 * p + 1;
                    p2 = 2 * p + 2;
                    if (InnerList.Count > p1 && OnCompare(p, p1) > 0) // links kleiner
                        p = p1;
                    if (InnerList.Count > p2 && OnCompare(p, p2) > 0) // rechts noch kleiner
                        p = p2;

                    if (p == pn)
                        break;
                    SwitchElements(p, pn);
                } while (true);

                return result;
            }

            /// <summary>
            /// Notify the PQ that the object at position i has changed
            /// and the PQ needs to restore order.
            /// </summary>
            public void Update(PathNodeKey item)
            {
                int count = InnerList.Count;

                // usually we only need to switch some elements, since estimation won't change that much.
                while ((item.Index - 1 >= 0) && (OnCompare(item.Index - 1, item.Index) > 0))
                {
                    SwitchElements(item.Index - 1, item.Index);
                }

                while ((item.Index + 1 < count) && (OnCompare(item.Index + 1, item.Index) < 0))
                {
                    SwitchElements(item.Index + 1, item.Index);
                }
            }

            /// <summary>
            /// Get the smallest object without removing it.
            /// </summary>
            /// <returns>The smallest object</returns>
            public PathNodeKey Peek()
            {
                if (InnerList.Count > 0)
                    return InnerList[0];
                return default(PathNodeKey);
            }

            public void Clear()
            {
                InnerList.Clear();
            }

            public int Count
            {
                get { return InnerList.Count; }
            }
        }

        private class SpaceTimeMap
        {
            private Map<PathNodeKey, IPathReservation> m_Entries = new Map<PathNodeKey, IPathReservation>(MapPolicy.None, PathNodeKey.Comparer);

            public IEnumerable<IPathReservation> GetPathList(System.Drawing.Point inCell)
            {
                PathNodeKey key = new PathNodeKey()
                {
                    X = inCell.X,
                    Y = inCell.Y,
                };

                return from e in m_Entries.Search(key, PathNodeKey.ComparerIgnoreTime) select e.Value;
            }

            public void Remove(PathNodeKey inKey)
            {
                m_Entries.Remove(inKey);
            }

            public IPathReservation this[PathNodeKey key]
            {
                get
                {
                    IPathReservation result;

                    m_Entries.TryGetValue(key, out result);

                    return result;
                }

                set
                {
                    if (m_Entries.ContainsKey(key))
                        m_Entries[key] = value;
                    else
                        m_Entries.Add(key, value);
                }
            }

            public void Clear()
            {
                m_Entries.Clear();
            }
        }
    }
}
